Adaptive Iterated Square-Root Cubature Kalman Filter and Its Application to SLAM of a Mobile Robot

For the mobile robot Simultaneous Localization and Mapping (SLAM),a new algorithm is proposed, and named Adaptive Iterated Square-Root Cubature Kalman Filter based SLAM algorithm (AISRCKF-SLAM). The main contribution of the algorithm is that the numerical integration method based on cubature rule is directly used to calculate the SLAM posterior probability density. To improve innovation covariance and cross-covariance, the latest measurements are iteratively used in the measurement updating. The algorithm can reduce linearization error and improve the accuracy of the SLAM algorithm. The algorithm also used adaptive iterating estimation restricted by the iterative sentencing guideline to adjust the proportion of the observation and dynamic model, to make the estimated square root of the error covariance more accurate and reasonable. In experiments, the proposed algorithm is compared with Extended Kalman Filter based SLAM algorithm (EKF-SLAM), Unscented Kalman Filter based SLAM algorithm (UKF-SLAM) and Square-Root Cubature Kalman Filter based SLAM algorithm (SRCKF-SLAM. The results indicate that the proposed algorithm having with the higher accuracy of the state estimation is obtained to compare with the EKF-SLAM algorithm, the UKFSLAM algorithm and the SRCKF-SLAM algorithm.